﻿#include "canopenbus.h"
#include "global.h"

//canbus库基于CanOpen协议
CanOpenBus::CanOpenBus()
{

}

void CanOpenBus::CanbusInit()
{
    canthread = new CANThread();
    connect(canthread,SIGNAL(getProtocolData(VCI_CAN_OBJ *,unsigned int ,unsigned int)),this,SLOT(slot_GetProtocolData(VCI_CAN_OBJ *,unsigned int,unsigned int)));
    bool ret = canthread->openDevice(4,0,500);
    if(!ret){
        mylog<<"can device open false!";
        return;
    }
    ret = canthread->initCAN();
    if(!ret){
        mylog<<"init can false!";
        canthread->closeDevice();
        return;
    }
    ret = canthread->startCAN();
    if(!ret){
        mylog<<"start can false!";
        canthread->closeDevice();
        return;
    }

    canthread->start();
    mylog<<"can device init and start success!";

}
/***************************************************************
* 函数名称：sendCanFram
* 功能描述：发送一帧can报文
* 参数说明：framid ID号，data 发送的字节数组
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool CanOpenBus::sendCanFram(quint32 framid, uint8_t *data)
{
    if(!canthread)
        return false;

    return canthread->sendData(0,framid,0,0,data,sizeof(data));;
}
/***************************************************************
* 函数名称：SDO_Read
* 功能描述：通过SDO读某对象的值
* 参数说明：nodeid 站号，objectindix 对象索引
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool CanOpenBus::SDO_Read(const uint8_t& nodeid, const uint32_t& objectindex)
{
    uint8_t data[8] = {0};
    //SDO命令字
    data[0] = 0x40;
    //对象索引
    data[1] = (objectindex>>8)&0xff;
    data[2] = (objectindex>>16)&0xff;
    data[3] = (objectindex)&0xff;

    // 将 uint8_t 数组转换为 QByteArray
    QByteArray byteArray(reinterpret_cast<const char*>(data), sizeof(data));
//    mylog<<"send data:"<<nodeid<<"  "<<byteArray.toHex();

    //如果发送成功
    return sendCanFram(0x600+nodeid,data);

            //取数据
//            returndata.recvFrame = canDevice->readFrame();
//            mylog<<"recv data:"<<hex<<returndata.recvFrame.frameId()<<"    "<<hex<<returndata.recvFrame.payload().toHex();
//            //根据返回内容读取正确的数据
//            if(returndata.recvFrame.payload().at(0) == 0x4F ||
//                    returndata.recvFrame.payload().at(0) == 0x4B ||
//                    returndata.recvFrame.payload().at(0) == 0x43)
//            {
//                returndata.isSucceed = true;
//                returndata.NodeID = returndata.recvFrame.frameId()-0x580;
//                mylog<<"ok!!!";
//            }
//            else if(returndata.recvFrame.payload().at(0) == 0x80)
//            {
//                returndata.isSucceed = false;
//                returndata.errorCode = 0x80;
//            }
//            else
//            {
//                returndata.isSucceed = false;
//                returndata.errorCode = 0x02;
//            }

}
/***************************************************************
* 函数名称：SDO_Write
* 功能描述：通过SDO模式向从站发送写参数
* 参数说明：nodeid 从站站号 objectindex 对象索引（包括子索引） count 写参数类型大小 data 参数值
* 返回值说明：recvFrame -- 返回帧   isSucceed -- 是否成功  errorCode -- 错误码
***************************************************************/
bool CanOpenBus::SDO_Write(const uint8_t& nodeid, const uint32_t& objectindex, const uint8_t& count, int data)
{
    uint8_t senddata[8] = {0};

    if(count == 1){
        senddata[0] = 0x2F;
        senddata[4] = data;
    }
    else if(count == 2){
        senddata[0] = 0x2B;
        senddata[4] = data;
        senddata[5] = data>>8;
    }
    else if(count == 4){
        senddata[0] = 0x23;
        senddata[4] = data;
        senddata[5] = data>>8;
        senddata[6] = data>>16;
        senddata[7] = data>>24;
    }
    else{
        return false;
    }

    senddata[1] = (objectindex>>8)&0xff;
    senddata[2] = (objectindex>>16)&0xff;
    senddata[3] = (objectindex)&0xff;

    // 将 uint8_t 数组转换为 QByteArray
    QByteArray byteArray(reinterpret_cast<const char*>(senddata), sizeof(senddata));
    mylog<<"send id:"<<hex<<0x600+nodeid<<" ,data:"<<nodeid<<"  "<<byteArray.toHex();

    return sendCanFram(0x600+nodeid,senddata);

//            returndata.recvFrame = canDevice->readFrame();
//            mylog<<"recv data:"<<hex<<returndata.recvFrame.frameId()<<"    "<<hex<<returndata.recvFrame.payload().toHex();
//            if(returndata.recvFrame.payload().at(0) == 0x60)
//            {
//                returndata.isSucceed = true;
//                returndata.NodeID = returndata.recvFrame.frameId() - 0x580;
//            }
//            else if(returndata.recvFrame.payload().at(0) == 0x80)
//            {
//                returndata.isSucceed = false;
//                returndata.errorCode = 0x80;
//                returndata.msg = (returndata.recvFrame.payload()[7]<<24)|(returndata.recvFrame.payload()[6]<<16)|(returndata.recvFrame.payload()[5]<<8)|(returndata.recvFrame.payload()[4]);
//            }
//            else
//            {
//                returndata.isSucceed = false;
//                returndata.errorCode = 0x02;
//            }
}

bool CanOpenBus::PDO_Write(uint8_t nodeid, uint32_t PDOx, uint8_t *data)
{
    uint32_t frameid = nodeid+PDOx;
    mylog<<"send PDO id:"<<hex<<frameid<<"  data:"<<hex<<data;
    return sendCanFram(frameid,data);
}
/***************************************************************
* 函数名称：MNT_Set
* 功能描述：发送MNT节点管理报文
* 参数说明：nodeid 从设备节点号   cmd 设定的状态
* 返回值说明：是否发送成功
***************************************************************/
bool CanOpenBus::MNT_Set(uint8_t nodeid, uint8_t cmd)
{
    uint8_t senddata[8];

    senddata[0] = cmd;
    senddata[1] = nodeid;
    senddata[2] = 0x00;
    senddata[3] = 0x00;
    senddata[4] = 0x00;
    senddata[5] = 0x00;
    senddata[6] = 0x00;
    senddata[7] = 0x00;
//    mylog<<senddata;
    return sendCanFram(0x0000,senddata);
}

bool CanOpenBus::Sync_Set()
{
    uint8_t  senddata[8];

    senddata[0] = 0x00;
    senddata[1] = 0x00;
    senddata[2] = 0x00;
    senddata[3] = 0x00;
    senddata[4] = 0x00;
    senddata[5] = 0x00;
    senddata[6] = 0x00;
    senddata[7] = 0x00;
//    mylog<<senddata;
    return sendCanFram(CANOPENCOBID::Sync_COBID,senddata);
}

FrameType CanOpenBus::determineFrameType(uint32_t cob_id)
{
    // 确保COB-ID为11位标准格式
    cob_id &= 0x7FF;

    // 判断是否为SDO响应（服务器到客户端，默认范围0x580~0x5FF）
    if (cob_id >= 0x580 && cob_id <= 0x5FF) {
        return FrameType::SDO_RESPONSE;
    }
    // 判断是否为SDO请求（客户端到服务器，默认范围0x600~0x67F）
    else if (cob_id >= 0x600 && cob_id <= 0x67F) {
        return FrameType::SDO_REQUEST;
    }
    // 判断是否为PDO（默认范围0x180~0x57F）
    else if (cob_id >= 0x180 && cob_id <= 0x57F) {
        return FrameType::PDO;
    }
    // 其他类型（如NMT、EMCY等）
    else {
        return FrameType::OTHER;
    }
}

void CanOpenBus::slot_GetProtocolData(VCI_CAN_OBJ *vci, unsigned int dwRel, unsigned int channel)
{
    QStringList messageList;
    QString str;
    for(unsigned int i = 0;i < dwRel;i ++)
    {
        QByteArray byteArray(reinterpret_cast<const char*>(vci[i].Data), sizeof(vci[i].Data));

        FrameType frametp = determineFrameType(vci[i].ID);
        switch (frametp) {
        case FrameType::SDO_REQUEST:
            break;
        case FrameType::SDO_RESPONSE:
        {
//            mylog<<"recv SDO_RESPONSE canopen id:"<<hex<<vci[i].ID<<" ,msg:"<<byteArray.toHex();
            CanopenSDOFrame sdoframe;
            sdoframe.NodeID = vci[i].ID - 0x580;
            sdoframe.cmdWord = vci[i].Data[0];

            sdoframe.objectMainIndex = (vci[i].Data[2]<<8)|(vci[i].Data[1]);
            sdoframe.objectSubIndex = vci[i].Data[3];
            sdoframe.objectIndex = (sdoframe.objectMainIndex<<8 | sdoframe.objectSubIndex);

            if(sdoframe.cmdWord != 0x80){
                if(sdoframe.cmdWord == SDORECVCMDWORD::Recv_1byte){
                    sdoframe.data = vci[i].Data[4];
                }
                else if(sdoframe.cmdWord == SDORECVCMDWORD::Recv_2byte){
                    sdoframe.data = (vci[i].Data[5]<<8)|(vci[i].Data[4]);
                }
                else{
                    sdoframe.data = (vci[i].Data[7]<<24)|(vci[i].Data[6]<<16)|(vci[i].Data[5]<<8)|(vci[i].Data[4]);
                }
            }
            emit sig_recvSDOFrame(sdoframe);
        }
            break;
        case FrameType::PDO:
        {
            mylog<<"recv PDO canopen id:"<<hex<<vci[i].ID<<" ,msg:"<<byteArray.toHex();

            CanopenPDOFram pdoframe;
            pdoframe.cobID = vci[i].ID;
            for(int j=0;j<vci[i].DataLen;++j){
                pdoframe.data[j] = vci[i].Data[j];
            }
            emit sig_recvPDOFrame(pdoframe);
        }
            break;
        case FrameType::OTHER:
            mylog<<"recv other canopen id:"<<hex<<vci[i].ID<<" ,msg:"<<byteArray.toHex();

            break;
        }

    }
}
